﻿using HidApi;
using MySoftLab.DapFlashCore.Probes.DapAccess;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Runtime.Intrinsics.Arm;
using System.Text;
using System.Threading.Tasks;

namespace MySoftLab.DapFlashCore.CoreSight
{
    public class DebugPort
    {
        private readonly DapAccessCmsisDap _link;
        // private Dictionary<uint, uint> _csw = new Dictionary<uint, uint>();
        private int _dpSelected = -1;
        private int _accessNumber = 0;
        private uint _dpIdr;
        private uint _dpVersion;
        private bool _isMindp;

        public List<APAddressBase> ValidAps { get; set; } = new List<APAddressBase>();

        public Dictionary<APAddressBase, AccessPort> Aps { get; set; } = new Dictionary<APAddressBase, AccessPort>();

        //public static Dictionary<string, DAPAccessIntf.REG> DP_REG = new Dictionary<string, DAPAccessIntf.REG>()
        //{
        //    { "IDCODE", DAPAccessIntf.REG.DP_0x0 },
        //    { "ABORT", DAPAccessIntf.REG.DP_0x0 },
        //    { "CTRL_STAT", DAPAccessIntf.REG.DP_0x4 },
        //    { "SELECT", DAPAccessIntf.REG.DP_0x8 },
        //};

        public static Dictionary<string, byte> AP_REG = new Dictionary<string, byte>()
        {
            { "CSW" , 0x00 },
            { "TAR" , 0x04 },
            { "DRW" , 0x0C },
            { "IDR" , 0xFC },
        };

        public const int _RESET_RECOVERY_ATTEMPTS_BEFORE_RECONNECT = 1;

        public const uint DP_IDR = 0x00; // read-only
        public const uint DP_IDR1 = 0x10; // read-only
        public const uint DP_BASEPTR0 = 0x20; // read-only
        public const uint DP_BASEPTR1 = 0x30; // read-only
        public const uint DP_ABORT = 0x00; // write-only
        public const uint DP_CTRL_STAT = 0x04; // read-write
        public const uint DP_DLCR = 0x14; // read-write
        public const uint DP_TARGETID = 0x24; // read-only
        public const uint DP_DLPIDR = 0x34; // read-only
        public const uint DP_EVENTSTAT = 0x44; // read-only
        public const uint DP_SELECT1 = 0x54; // write-only
        public const uint DP_SELECT = 0x8; // write-only
        public const uint DP_RDBUFF = 0xC; // read-only

        public const uint DPADDR_MASK = 0x0f;

        public static uint CTRLSTAT_STICKYORUN = 0x00000002;
        public static uint CTRLSTAT_STICKYCMP = 0x00000010;
        public static uint CTRLSTAT_STICKYERR = 0x00000020;

        public static uint IDCODE = 0 << 2;
        public static uint AP_ACC = 1 << 0;
        public static uint DP_ACC = 0 << 0;
        public static uint READ = 1 << 1;
        public static uint WRITE = 0 << 1;
        public static uint VALUE_MATCH = 1 << 4;
        public static int MATCH_MASK = 1 << 5;

        public static uint A32 = 0x0c;
        public static int APSEL_SHIFT = 24;
        public static uint APSEL = 0xff000000;
        public static uint APBANKSEL = 0x000000f0;
        public static uint APREG_MASK = 0x000000fc;
        public static uint APSEL_APBANKSEL = APSEL | APBANKSEL;

        public const uint DPIDR_REVISION_MASK = 0xf0000000;
        public const int DPIDR_REVISION_SHIFT = 28;
        public const uint DPIDR_PARTNO_MASK = 0x0ff00000;
        public const int DPIDR_PARTNO_SHIFT = 20;
        public static uint DPIDR_MIN_MASK = 0x10000;
        public static uint DPIDR_VERSION_MASK = 0xf000;
        public static int DPIDR_VERSION_SHIFT = 12;

        public static uint CSYSPWRUPACK = 0x80000000;
        public static uint CDBGPWRUPACK = 0x20000000;
        public static uint CSYSPWRUPREQ = 0x40000000;
        public static uint CDBGPWRUPREQ = 0x10000000;

        public static uint TRNNORMAL = 0x00000000;
        public static uint MASKLANE = 0x00000f00;

        public const uint ABORT_DAPABORT = 0x00000001;
        public const uint ABORT_STKCMPCLR = 0x00000002;
        public const uint ABORT_STKERRCLR = 0x00000004;
        public const uint ABORT_WDERRCLR = 0x00000008;
        public const uint ABORT_ORUNERRCLR = 0x00000010;

        // Mask and shift for extracting DPBANKSEL from our DP register address constants. These are not
        // related to the SELECT.DPBANKSEL bitfield.
        public const uint DPADDR_DPBANKSEL_MASK = 0xf0;
        public const int DPADDR_DPBANKSEL_SHIFT = 4;

        // # DP SELECT register fields.
        public const uint SELECT_DPBANKSEL_MASK = 0x0000000f;
        public const uint SELECT_APADDR_MASK = 0xfffffff0;

        private uint? _cachedDpSelect = null;

        public DebugPort(DapAccessCmsisDap link)
        {
            _link = link;
        }

        public int NextAccessNumber
        {
            get
            {
                _accessNumber += 1;
                return _accessNumber;
            }
        }

        public DapAccessCmsisDap Link => _link;

        public void Init()
        {
            // Connect to the target
            _link.Connect();
            InitSequence();
            ReadIdCode();
            ClearStickyErr();
        }

        /// <summary>
        /// Read IDR register and get DP version
        /// </summary>
        /// <returns></returns>
        public DPIDR ReadIdCode()
        {
            // Read ID register and get DP version
            var dpidr = _link.ReadDP(DP_IDR).Value;
            Console.WriteLine($"DPConnector read DP_IDR = 0x{dpidr:X8}");
            var dpPartno = (dpidr & DPIDR_PARTNO_MASK) >> DPIDR_PARTNO_SHIFT;
            var dpVersion = (dpidr & DPIDR_VERSION_MASK) >> DPIDR_VERSION_SHIFT;
            var dpRevision = (dpidr & DPIDR_REVISION_MASK) >> DPIDR_REVISION_SHIFT;
            var isMindp = (dpidr & DPIDR_MIN_MASK) != 0;
            return new DPIDR(dpidr, dpPartno, dpVersion, dpRevision, isMindp);
        }

        private void LineReset()
        {
            _link.SwjSequence(51, 0xffffffffffffff);
        }

        private void InitSequence()
        {
            // 初始化时序
            LineReset();

            _link.SwjSequence(16, 0xe79e);
            LineReset();
            _link.SwjSequence(8, 0x00);
        }


        public void Flush()
        {
            try
            {
                _link.Flush();
            }
            finally
            {
                _dpSelected = -1;
            }
        }

        public DelayResult<uint> ReadReg(uint addr, bool now = true)
        {
            return ReadDP(addr, now);
        }

        public void WriteReg(uint addr, uint data)
        {
            WriteDP(addr, data);
        }

        /// <summary>
        /// Assert DP power requests
        /// Request both debug and system power be enabled, and wait until the request is acked.
        /// There is a timeout for the request.
        /// </summary>
        /// <returns>Boolean indicating whether the power up request succeeded.</returns>
        public bool PowerUpDebug()
        {
            // Send power up request for system and debug.
            WriteReg(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ | MASKLANE | TRNNORMAL);

            bool isTimeout = true;
            var start = DateTime.Now;
            while (DateTime.Now - start < TimeSpan.FromSeconds(5))
            {
                var r = ReadReg(DP_CTRL_STAT).Value;
                if ((r & (CDBGPWRUPACK | CSYSPWRUPACK)) == (CDBGPWRUPACK | CSYSPWRUPACK))
                {
                    isTimeout = false;
                    break;
                }
            }

            return isTimeout == false;
        }

        /// <summary>
        /// Deassert DP power requests.
        /// ADIv6 says that we must not clear CSYSPWRUPREQ and CDBGPWRUPREQ at the same time.
        /// ADIv5 says CSYSPWRUPREQ must not be set to 1 while CDBGPWRUPREQ is set to 0. So we
        /// start with deasserting system power, then debug power. Each deassertion has its own
        /// timeout.
        /// </summary>
        /// <returns>Boolean indicating whether the power down request succeeded.</returns>
        public bool PowerDownDebug()
        {
            // # Power down system first.
            WriteReg(DP_CTRL_STAT, CDBGPWRUPREQ | MASKLANE | TRNNORMAL);

            bool isTimeout = true;
            var start = DateTime.Now;
            while (DateTime.Now - start < TimeSpan.FromSeconds(5))
            {
                var r = ReadReg(DP_CTRL_STAT).Value;
                if ((r & (CDBGPWRUPACK | CSYSPWRUPACK)) == CDBGPWRUPACK)
                {
                    isTimeout = false;
                    break;
                }
            }

            if (isTimeout)
            {
                return false;
            }

            // # Now power down debug.
            WriteReg(DP_CTRL_STAT, MASKLANE | TRNNORMAL);
            isTimeout = true;
            start = DateTime.Now;
            while (DateTime.Now - start < TimeSpan.FromSeconds(5))
            {
                var r = ReadReg(DP_CTRL_STAT).Value;
                if ((r & (CDBGPWRUPACK | CSYSPWRUPACK)) == 0)
                {
                    isTimeout = false;
                    break;
                }
            }

            return isTimeout == false;
        }

        public void WriteApMultiple(uint addr, uint[] values)
        {
            var num = NextAccessNumber;

            try
            {
                SelectAP(addr);
                _link.WriteApMultiple(addr, values);
            }
            catch (Exception ex)
            {
                HandleError(ex, num);
                throw;
            }
        }

        public DelayResult<uint[]> ReadApMultiple(uint addr, int count, bool now = true)
        {
            var num = NextAccessNumber;

            DelayResult<uint[]>? resultCb = null;

            try
            {
                SelectAP(addr);

                resultCb = _link.ReadApMultiple(addr, count, false);
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.ToString());
            }

            // Need to wrap the deferred callback to convert exceptions.
            var readApMultipleCb = () =>
            {
                try
                {
                    if (resultCb != null && resultCb.Action != null)
                    {
                        return resultCb.Action();
                    }
                    else
                    {
                        return new uint[0];
                    }
                }
                catch (Exception ex)
                {
                    Console.WriteLine(ex.ToString());
                    return new uint[0];
                }
            };

            if (now)
            {
                return new DelayResult<uint[]>(readApMultipleCb());
            }
            else
            {
                return new DelayResult<uint[]>(readApMultipleCb);
            }
        }

        public void Reset()
        {
            try
            {
                _link.Reset();
                PostResetRecovery();
            }
            finally
            {
                _dpSelected = -1;
            }
        }

        public void AssertReset(bool asserted)
        {
            _link.AssertReset(asserted);
            _dpSelected = -1;
        }

        /// <summary>
        /// Wait for the target to recover from reset, with auto-reconnect if needed.
        /// </summary>
        public void PostResetRecovery()
        {
            var startTime = DateTime.Now;
            int attempt = 0;

            bool isTimeout = true;

            while (DateTime.Now - startTime < TimeSpan.FromSeconds(2))
            {
                try
                {
                    // Try to read CTRL/STAT. If the power-up bits request are reset, then the DP
                    // connection was not lost and we can just return.
                    var value = ReadReg(DP_CTRL_STAT).Value;
                    if ((value & (CSYSPWRUPREQ | CDBGPWRUPREQ)) == (CSYSPWRUPREQ | CDBGPWRUPREQ))
                    {
                        isTimeout = false;
                        return;
                    }
                }
                catch (DAPAccessIntf.TransferError)
                {
                    try
                    {
                        Flush();
                    }
                    catch (DAPAccessIntf.TransferError)
                    {

                    }
                }

                if (attempt == _RESET_RECOVERY_ATTEMPTS_BEFORE_RECONNECT)
                {
                    Console.WriteLine("DAP is not accessible after reset; attempting reconnect");
                }
                else if (attempt > _RESET_RECOVERY_ATTEMPTS_BEFORE_RECONNECT)
                {
                    // try reconnect
                    try
                    {
                        Init();
                    }
                    catch (Exception)
                    {
                    }
                }

                attempt += 1;
            }

            if (isTimeout)
            {
                Console.WriteLine("DAP is not accessible after reset followed by attempted reconnect");
            }
        }

        public void SetClock(int frequency)
        {
            _link.SetClock(frequency);
        }

        public void FindAps()
        {
            uint apNum = 0;

            while (true)
            {
                try
                {
                    var delayedIdr = ReadAP(apNum << APSEL_SHIFT | AP_REG["IDR"]);

                    var idr = delayedIdr.Value;
                    if (idr == 0)
                    {
                        break;
                    }
                }
                catch (Exception)
                {
                    break;
                }

                apNum += 1;
            }
        }

        public DelayResult<uint> ReadDP(uint addr, bool now = true)
        {
            if ((addr & DPADDR_MASK) % 4 != 0)
            {
                throw new Exception("DP address must be word aligned");
            }

            var num = NextAccessNumber;

            // # Update DPBANKSEL if required.
            SetDpBankSel(addr, false);

            DelayResult<uint> resultCb;
            try
            {
                resultCb = _link.ReadDP(addr & DPADDR_MASK, false);
            }
            catch (Exception ex)
            {
                HandleError(ex, num);
                throw;
            }

            // Read callback returned for async reads.
            Func<uint> readDPCb = () =>
            {
                try
                {
                    uint result;

                    if (resultCb.IsValue)
                    {
                        result = resultCb.Value;
                    }
                    else
                    {
                        if (resultCb.Action != null)
                        {
                            result = resultCb.Action();
                        }
                        else
                        {
                            throw new Exception();
                        }
                    }

                    return result;
                }
                catch (Exception ex)
                {
                    HandleError(ex, num);
                    throw;
                }
            };

            if (now)
            {
                return new DelayResult<uint>(readDPCb());
            }
            else
            {
                return new DelayResult<uint>(readDPCb);
            }
        }

        private void SetDpBankSel(uint addr, bool isWrite)
        {
            uint[] registersIgnoringDpbanksel = new uint[] { DP_ABORT, DP_SELECT, DP_RDBUFF };

            if (!registersIgnoringDpbanksel.Contains(addr & DPADDR_MASK))
            {
                // Get the DP bank.
                var dpbanksel = (addr & DPADDR_DPBANKSEL_MASK) >> DPADDR_DPBANKSEL_SHIFT;

                WriteDpSelect(SELECT_DPBANKSEL_MASK, dpbanksel);
            }
        }

        public bool WriteDP(uint addr, uint data)
        {
            if ((addr & DPADDR_MASK) % 4 != 0)
            {
                throw new ArgumentException("DP address must be word aligned");
            }

            var num = NextAccessNumber;

            // Update DPBANKSEL if required.
            SetDpBankSel(addr, true);

            // Write the DP register.
            try
            {
                _link.WriteDP(addr & DPADDR_MASK, data);
            }
            catch (Exception ex)
            {
                HandleError(ex, num);
                throw;
            }

            return true;
        }

        public DAPAccessIntf.REG ApAddrToReg(uint addr)
        {
            return (DAPAccessIntf.REG)(4 + ((addr & A32) >> 2));
        }

        private void WriteDpSelect(uint mask, uint value)
        {
            uint select;
            if (_cachedDpSelect == null)
            {
                select = value;
            }
            else
            {
                select = _cachedDpSelect.Value & ~mask | value;

                if (select == _cachedDpSelect.Value)
                {
                    return;
                }
            }

            // Update the SELECT register and cache.
            WriteDP(DP_SELECT, select);
            _cachedDpSelect = select;
        }

        private bool SelectAP(uint addr)
        {
            Lock();
            WriteDpSelect(APSEL_APBANKSEL, addr & APSEL_APBANKSEL);
            return true;
        }

        public bool WriteAP(uint addr, uint data)
        {
            var num = NextAccessNumber;

            bool didLock = false;

            didLock = SelectAP(addr);

            // Write AP
            _link.WriteAP(addr, data);

            return true;
        }

        public DelayResult<uint> ReadAP(uint addr, bool now = true)
        {
            var num = NextAccessNumber;

            DelayResult<uint> resultCb;

            try
            {
                SelectAP(addr);
                resultCb = _link.ReadAP(addr, false);
            }
            catch (Exception ex)
            {
                HandleError(ex, num);
                throw;
            }

            // Read callback returned for async reads.
            Func<uint> readApCb = () =>
            {
                try
                {
                    uint result;

                    if (resultCb.IsValue)
                    {
                        result = resultCb.Value;
                    }
                    else
                    {
                        if (resultCb.Action != null)
                        {
                            result = resultCb.Action();
                        }
                        else
                        {
                            throw new Exception();
                        }
                    }

                    return result;
                }
                catch (Exception ex)
                {
                    HandleError(ex, num);
                    throw;
                }
            };

            if (now)
            {
                return new DelayResult<uint>(readApCb());
            }
            else
            {
                return new DelayResult<uint>(readApCb);
            }
        }

        public void HandleError(Exception error, int num)
        {
            _dpSelected = -1;

            if (error is DAPAccessIntf.TransferFaultError)
            {
                ClearStickyErr();
            }
        }

        private void InvalidateCache()
        {
            _cachedDpSelect = null;
        }

        public bool ClearStickyErr()
        {
            InvalidateCache();
            var mode = _link.GetSwjMode();
            if (mode == DAPAccessIntf.PORT.SWD)
            {
                // TODO check
                WriteReg(DP_ABORT, ABORT_ORUNERRCLR | ABORT_WDERRCLR | ABORT_STKERRCLR | ABORT_STKCMPCLR);
                return true;
            }
            else if (mode == DAPAccessIntf.PORT.JTAG)
            {
                // _link.WriteReg((DAPAccessIntf.REG)DP_REG["CTRL_STAT"], CTRLSTAT_STICKYERR);
                return true;
            }
            else
            {
                return false;
            }
        }

        private void Lock()
        {
            _link.Lock();
        }

        private void Unlock()
        {
            _link.Unlock();
        }
    }
}
